Sigma - Point Kalman Filters for Nonlinear Estimation and Sensor - Fusion - Applications to Integrated Navigation - Rudolph

Nonlinear estimation based on probabilistic inference forms a core component in most modern GNC systems. The estimator optimally fuses observations from multiple sensors with predictions from a nonlinear dynamic state-space model of the system under control. The current industry standard and most widely used algorithm for this purpose is the extended Kalman filter (EKF). Unfortunately, the EKF is based on a sub-optimal implementation of the recursive Bayesian estimation framework applied to Gaussian random variables. This can seriously affect the accuracy or even lead to divergence of the system. In this paper, we apply a new probabilistic framework, called Sigma-point Kalman Filters (SPKF), to the same problem domain typically addressed by the EKF. SPKF methods have proven to be far superior to standard EKF based estimation approaches in a wide range of applications. Whereas the EKF can be viewed as a first-order method for dealing with nonlinearities, an SPKF achieves second-order or higher accuracy. Remarkably, the computational complexity of a SPKF is of the same order as the EKF. Furthermore, implementation of the SPKF is often substantially easier and requires no analytic derivation or Jacobians as in the EKF. In this paper, we review the fundamental development of the SPKF family of algorithms. These include specific variants such as the unscented Kalman filter (UKF), the central-difference Kalman filter (CDKF), and numerically efficient and stable square-root implementations. We also introduce a novel SPKF based method to fuse latency lagged observations in a theoretically consistent fashion. In the second part of the paper, we focus on the application of the SPKF to the integrated navigation problem as it relates to unmanned aerial vehicle (UAV) autonomy. We specifically detail the development of a loosely coupled implementation for integrating GPS measurements with an inertial measurement unit (IMU) and altimeter. The SPKFbased sensor latency compensation technique mentioned above is used to accurately fuse the lagged GPS measurements. A UAV (helicopter) test platform is used to demonstrate the results. Performance metrics indicate an approximate 30% error reduction in both attitude and position estimates relative to the baseline EKF implementation.